#include "CarSoloMeshBuilder.h"
#include "DetourNavMesh.h"
#include "DetourNavMeshQuery.h"
#include "DetourNavMeshBuilder.h"
#include <string.h>
#include <math.h>
#include "CarDebug.h"
#include "RecastAlloc.h"

enum CarPartitionType
{
	SAMPLE_PARTITION_WATERSHED,
	SAMPLE_PARTITION_MONOTONE,
	SAMPLE_PARTITION_LAYERS,
};

enum CarPolyAreas
{
	SAMPLE_POLYAREA_GROUND,
	SAMPLE_POLYAREA_WATER,
	SAMPLE_POLYAREA_ROAD,
	SAMPLE_POLYAREA_DOOR,
	SAMPLE_POLYAREA_GRASS,
	SAMPLE_POLYAREA_JUMP,
};

enum CarPolyFlags
{
	SAMPLE_POLYFLAGS_WALK = 0x01,		// Ability to walk (ground, grass, road)
	SAMPLE_POLYFLAGS_SWIM = 0x02,		// Ability to swim (water).
	SAMPLE_POLYFLAGS_DOOR = 0x04,		// Ability to move through doors.
	SAMPLE_POLYFLAGS_JUMP = 0x08,		// Ability to jump.
	SAMPLE_POLYFLAGS_DISABLED = 0x10,		// Disabled polygon
	SAMPLE_POLYFLAGS_ALL = 0xffff	// All abilities.
};

CarSoloMeshBuilder CarSoloMeshBuilder::instance;

CarSoloMeshBuilder::CarSoloMeshBuilder()
{
	mNavMeshQuery = dtAllocNavMeshQuery();
}

CarSoloMeshBuilder::~CarSoloMeshBuilder()
{
	Cleanup();
}

void CarSoloMeshBuilder::SetBuildConfig(CarBuildConfig* carBuildConfig)
{
	mCarBuildConfig = carBuildConfig;
}

CarBuildConfig* CarSoloMeshBuilder::GetBuildConfig()
{
	return mCarBuildConfig;
}

void CarSoloMeshBuilder::SetMeshData(float* vertexBuffer, int vertexCount, int* triangleBuffer, int triangleCount)
{
	if (mCarBuildConfig != nullptr)
	{
		int vertexBufferSize = vertexCount * 3 * sizeof(float);
		mCarBuildConfig->vertexBuffer = (float*)rcAlloc(vertexBufferSize, RC_ALLOC_PERM);
		memcpy(mCarBuildConfig->vertexBuffer, vertexBuffer, vertexBufferSize);
		mCarBuildConfig->vertexCount = vertexCount;

		int triangleBufferSize = triangleCount * 3 * sizeof(int);
		mCarBuildConfig->triangleBuffer = (int*)rcAlloc(triangleBufferSize, RC_ALLOC_PERM);
		memcpy(mCarBuildConfig->triangleBuffer, triangleBuffer, triangleBufferSize);
		mCarBuildConfig->triangleCount = triangleCount;
	}
}

bool CarSoloMeshBuilder::Build()
{
	memset(&mConfig, 0, sizeof(mConfig));

	mConfig.cs = mCarBuildConfig->cs;
	mConfig.ch = mCarBuildConfig->ch;
	mConfig.walkableSlopeAngle = mCarBuildConfig->agentMaxSlope;
	mConfig.walkableHeight = (int)ceilf(mCarBuildConfig->agentHeight / mConfig.ch);
	mConfig.walkableClimb = (int)floorf(mCarBuildConfig->agentMaxClimb / mConfig.ch);
	mConfig.walkableRadius = (int)ceilf(mCarBuildConfig->agentRadius / mConfig.cs);
	mConfig.maxEdgeLen = (int)(mCarBuildConfig->maxEdgeLen / mCarBuildConfig->cs);
	mConfig.maxSimplificationError = mCarBuildConfig->maxSimplificationError;
	mConfig.minRegionArea = (int)rcSqr(mCarBuildConfig->minRegionArea);
	mConfig.mergeRegionArea = (int)rcSqr(mCarBuildConfig->mergeRegionArea);
	mConfig.maxVertsPerPoly = (int)mCarBuildConfig->maxVertsPerPoly;
	mConfig.detailSampleDist = mCarBuildConfig->detailSampleDist < 0.9f ? 0 : mCarBuildConfig->cs * mCarBuildConfig->detailSampleDist;
	mConfig.detailSampleMaxError = mCarBuildConfig->ch * mCarBuildConfig->detailSampleMaxError;

	rcVcopy(mConfig.bmin, mCarBuildConfig->bmin);
	rcVcopy(mConfig.bmax, mCarBuildConfig->bmax);
	rcCalcGridSize(mConfig.bmin, mConfig.bmax, mConfig.cs, &mConfig.width, &mConfig.height);

	mHeightfield = rcAllocHeightfield();
	if (!mHeightfield)
	{
		CarDebug::instance.LogError(L"buildNavigation: Out of memory height field");
		return false;
	}

	if (!rcCreateHeightfield(&mContext, *mHeightfield, mConfig.width, mConfig.height, mConfig.bmin, mConfig.bmax, mConfig.cs, mConfig.ch))
	{
		CarDebug::instance.Log(L"buildNavigation: Could not create solid heightfield");
		return false;
	}

	mTriangleAreas = new unsigned char[mCarBuildConfig->triangleCount];
	if (!mTriangleAreas)
	{
		CarDebug::instance.Log(L"buildNavigation: Out of memory triangle areas");
		return false;
	}

	memset(mTriangleAreas, 0, mCarBuildConfig->triangleCount*sizeof(unsigned char));
	rcMarkWalkableTriangles(&mContext, mConfig.walkableSlopeAngle, mCarBuildConfig->vertexBuffer, mCarBuildConfig->vertexCount, mCarBuildConfig->triangleBuffer, mCarBuildConfig->triangleCount, mTriangleAreas);
	if (!rcRasterizeTriangles(&mContext, mCarBuildConfig->vertexBuffer, mCarBuildConfig->vertexCount, mCarBuildConfig->triangleBuffer, mTriangleAreas, mCarBuildConfig->triangleCount, *mHeightfield, mConfig.walkableClimb))
	{
		CarDebug::instance.Log(L"buildNavigation: Could not rasterize triangles");
		return false;
	}

	if (!mKeepInternalResults)
	{
		delete[] mTriangleAreas;
		mTriangleAreas = nullptr;
	}

	rcFilterLowHangingWalkableObstacles(&mContext, mConfig.walkableClimb, *mHeightfield);
	rcFilterLedgeSpans(&mContext, mConfig.walkableHeight, mConfig.walkableClimb, *mHeightfield);
	rcFilterWalkableLowHeightSpans(&mContext, mConfig.walkableHeight, *mHeightfield);

	mCompactHeightfield = rcAllocCompactHeightfield();
	if (!mCompactHeightfield)
	{
		CarDebug::instance.Log(L"buildNavigation: Out of memory compact height field");
		return false;
	}
	if (!rcBuildCompactHeightfield(&mContext, mConfig.walkableHeight, mConfig.walkableClimb, *mHeightfield, *mCompactHeightfield))
	{
		CarDebug::instance.Log(L"buildNavigation: Could not build compact data");
		return false;
	}

	if (!mKeepInternalResults)
	{
		rcFreeHeightField(mHeightfield);
		mHeightfield = 0;
	}

	if (!rcErodeWalkableArea(&mContext, mConfig.walkableRadius, *mCompactHeightfield))
	{
		CarDebug::instance.Log(L"buildNavigation: Could not erode");
		return false;
	}

	if (mCarBuildConfig->partitionType == SAMPLE_PARTITION_WATERSHED)
	{
		if (!rcBuildDistanceField(&mContext, *mCompactHeightfield))
		{
			CarDebug::instance.Log(L"buildNavigation: Could not build distance field");
			return false;
		}

		if (!rcBuildRegions(&mContext, *mCompactHeightfield, 0, mConfig.minRegionArea, mConfig.mergeRegionArea))
		{
			CarDebug::instance.Log(L"buildNavigation: Could not build watershed regions");
			return false;
		}
	}
	else if (mCarBuildConfig->partitionType == SAMPLE_PARTITION_MONOTONE)
	{
		if (!rcBuildRegionsMonotone(&mContext, *mCompactHeightfield, 0, mConfig.minRegionArea, mConfig.mergeRegionArea))
		{
			CarDebug::instance.Log(L"buildNavigation: Could not build monotone regions");
			return false;
		}
	}
	else
	{
		if (!rcBuildLayerRegions(&mContext, *mCompactHeightfield, 0, mConfig.minRegionArea))
		{
			CarDebug::instance.Log(L"buildNavigation: Could not build layer regions");
			return false;
		}
	}

	mContourSet = rcAllocContourSet();
	if (!mContourSet)
	{
		CarDebug::instance.Log(L"buildNavigation: Out of memory contour set");
		return false;
	}
	if (!rcBuildContours(&mContext, *mCompactHeightfield, mConfig.maxSimplificationError, mConfig.maxEdgeLen, *mContourSet))
	{
		CarDebug::instance.Log(L"buildNavigation: Could not create contours");
		return false;
	}

	mPolyMesh = rcAllocPolyMesh();
	if (!mPolyMesh)
	{
		CarDebug::instance.Log(L"buildNavigation: Out of memory pmesh");
		return false;
	}
	if (!rcBuildPolyMesh(&mContext, *mContourSet, mConfig.maxVertsPerPoly, *mPolyMesh))
	{
		CarDebug::instance.Log(L"buildNavigation: Could not triangulate contours");
		return false;
	}

	mPolyMeshDetail = rcAllocPolyMeshDetail();
	if (!mPolyMeshDetail)
	{
		CarDebug::instance.Log(L"buildNavigation: Out of memory poly mesh detail");
		return false;
	}

	if (!rcBuildPolyMeshDetail(&mContext, *mPolyMesh, *mCompactHeightfield, mConfig.detailSampleDist, mConfig.detailSampleMaxError, *mPolyMeshDetail))
	{
		CarDebug::instance.Log(L"buildNavigation: Could not build detail mesh");
		return false;
	}

	if (!mKeepInternalResults)
	{
		rcFreeCompactHeightfield(mCompactHeightfield);
		mCompactHeightfield = 0;
		rcFreeContourSet(mContourSet);
		mContourSet = 0;
	}

	if (mConfig.maxVertsPerPoly <= DT_VERTS_PER_POLYGON)
	{
		unsigned char* navData = 0;
		int navDataSize = 0;

		for (int i = 0; i < mPolyMesh->npolys; ++i)
		{
			if (mPolyMesh->areas[i] == RC_WALKABLE_AREA)
				mPolyMesh->areas[i] = SAMPLE_POLYAREA_GROUND;

			if (mPolyMesh->areas[i] == SAMPLE_POLYAREA_GROUND ||
				mPolyMesh->areas[i] == SAMPLE_POLYAREA_GRASS ||
				mPolyMesh->areas[i] == SAMPLE_POLYAREA_ROAD)
			{
				mPolyMesh->flags[i] = SAMPLE_POLYFLAGS_WALK;
			}
			else if (mPolyMesh->areas[i] == SAMPLE_POLYAREA_WATER)
			{
				mPolyMesh->flags[i] = SAMPLE_POLYFLAGS_SWIM;
			}
			else if (mPolyMesh->areas[i] == SAMPLE_POLYAREA_DOOR)
			{
				mPolyMesh->flags[i] = SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_DOOR;
			}
		}

		dtNavMeshCreateParams params;
		memset(&params, 0, sizeof(params));
		params.verts = mPolyMesh->verts;
		params.vertCount = mPolyMesh->nverts;
		params.polys = mPolyMesh->polys;
		params.polyAreas = mPolyMesh->areas;
		params.polyFlags = mPolyMesh->flags;
		params.polyCount = mPolyMesh->npolys;
		params.nvp = mPolyMesh->nvp;
		params.detailMeshes = mPolyMeshDetail->meshes;
		params.detailVerts = mPolyMeshDetail->verts;
		params.detailVertsCount = mPolyMeshDetail->nverts;
		params.detailTris = mPolyMeshDetail->tris;
		params.detailTriCount = mPolyMeshDetail->ntris;
		params.offMeshConVerts = nullptr;
		params.offMeshConRad = nullptr;
		params.offMeshConDir = nullptr;
		params.offMeshConAreas = nullptr;
		params.offMeshConFlags = nullptr;
		params.offMeshConUserID = nullptr;
		params.offMeshConCount = 0;
		params.walkableHeight = mCarBuildConfig->agentHeight;
		params.walkableRadius = mCarBuildConfig->agentRadius;
		params.walkableClimb = mCarBuildConfig->agentMaxClimb;
		rcVcopy(params.bmin, mPolyMesh->bmin);
		rcVcopy(params.bmax, mPolyMesh->bmax);
		params.cs = mConfig.cs;
		params.ch = mConfig.ch;
		params.buildBvTree = true;

		if (!dtCreateNavMeshData(&params, &navData, &navDataSize))
		{
			CarDebug::instance.Log(L"Could not build Detour navmesh.");
			return false;
		}

		mNavMesh = dtAllocNavMesh();
		if (!mNavMesh)
		{
			dtFree(navData);
			CarDebug::instance.Log(L"Could not create Detour navmesh");
			return false;
		}

		dtStatus status;

		status = mNavMesh->init(navData, navDataSize, DT_TILE_FREE_DATA);
		if (dtStatusFailed(status))
		{
			dtFree(navData);
			CarDebug::instance.Log(L"Could not init Detour navmesh");
			return false;
		}

		status = mNavMeshQuery->init(mNavMesh, 2048);
		if (dtStatusFailed(status))
		{
			CarDebug::instance.Log(L"Could not init Detour navmesh query");
			return false;
		}
	}

	return true;
}

void CarSoloMeshBuilder::Cleanup()
{
	if (mTriangleAreas != nullptr)
	{
		delete[] mTriangleAreas;
		mTriangleAreas = nullptr;
	}

	if (mHeightfield != nullptr)
	{
		rcFreeHeightField(mHeightfield);
		mHeightfield = nullptr;
	}

	if (mCompactHeightfield != nullptr)
	{
		rcFreeCompactHeightfield(mCompactHeightfield);
		mCompactHeightfield = nullptr;
	}

	if (mContourSet != nullptr)
	{
		rcFreeContourSet(mContourSet);
		mContourSet = nullptr;
	}

	if (mPolyMesh != nullptr)
	{
		rcFreePolyMesh(mPolyMesh);
		mPolyMesh = nullptr;
	}

	if (mPolyMeshDetail != nullptr)
	{
		rcFreePolyMeshDetail(mPolyMeshDetail);
		mPolyMeshDetail = nullptr;
	}

	if (mNavMesh != nullptr)
	{
		dtFreeNavMesh(mNavMesh);
		mNavMesh = nullptr;
	}

	if (mCarBuildConfig != nullptr)
	{
		rcFree(mCarBuildConfig->vertexBuffer);
		rcFree(mCarBuildConfig->triangleBuffer);
	}
}

